import serial
import socket
import struct
import time
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Pose, Twist
from geometry_msgs.msg import PoseStamped,TransformStamped
from std_msgs.msg import Header
from rclpy.qos import qos_profile_sensor_data
import sys
import math
from tf2_ros import TransformBroadcaster

# 定义报文头和数据ID
HEAD = 0xA5
CRC16_POLY = 0x8005
EXPECTED_LENGTH = 61  # 预计的报文长度
EARTH_RADIUS = 6371008.7714150598

class PublisherNode(Node):

    def __init__(self):
        super().__init__('read_serial_new')
        self.get_logger().info("publisher_node has been started.")

        self.isSend = False

        self.droneid=int(sys.argv[3])
        self.setlon = float(sys.argv[4])
        self.setlat = float(sys.argv[5])
        self.setalt = float(sys.argv[6])
        self.data_id = -1
        self.origin_lon = -1.0
        self.origin_lat = -1.0
        self.origin_alt = -1.0
        self.origin_yaw = -1.0
        self.origin_roll = -1.0
        self.origin_pitch = -1.0
        self.frequency = 10
        self.data_field = None
        self.last_time = 0
        self.publisher_ = self.create_publisher(Float64MultiArray, f"drone{self.droneid}/velocity_and_position", 10)
        self.publisher_pos = self.create_publisher(PoseStamped, f"drone{self.droneid}/local_position/pose", qos_profile_sensor_data)
        # self.publisher_odom = self.create_publisher(Odometry, f"drone{self.droneid}/local_position/odom", qos_profile_sensor_data)
        self.publisher_odom = self.create_publisher(Odometry, "/odom", qos_profile_sensor_data)
        self.tf_broadcaster = TransformBroadcaster(self)
        print(str(sys.argv[1]))
        self.port = ('/dev/ttyACM'+str(sys.argv[1]))

        print(str(sys.argv[2]))
        self.server_address = (str(sys.argv[2]), 12345)
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        # self.sock.bind(self.server_address) # 绑定套接字到指定地址和端口


        #self.publisher_1 = self.create_publisher(Float64MultiArray, f"drone{self.droneid+1}/velocity_and_position", 10)
        #self.publisher_2 = self.create_publisher(Float64MultiArray, f"drone{self.droneid+2}/velocity_and_position", 10)
        #self.timer = self.create_timer(0.1, self.read_and_parse_serial_data)

    def publish_data(self, position, velocity, angular_velocity, local_p, quatation):
        # 创建消息对象
        msg = Float64MultiArray()
        msg_pose = PoseStamped()
        msg_odom = Odometry()
        flag = [8.8]
        # 设置消息数据，依次为三轴速度和三轴坐标
        msg.data = flag + position + velocity  # 合并速度和坐标，假设长度一致
        
        msg_pose.header.stamp = self.get_clock().now().to_msg()  # 设置时间戳
        msg_pose.header.frame_id = 'map'  # 设置坐标系

        # 设置位置
        # print(f"local_p: {local_p}")
        msg_pose.pose.position.x = local_p[0]
        msg_pose.pose.position.y = local_p[1]
        msg_pose.pose.position.z = local_p[2]

        # print(f"quation: {quatation}")
        msg_pose.pose.orientation.x = quatation[0]
        msg_pose.pose.orientation.y = quatation[1]
        msg_pose.pose.orientation.z = quatation[2]
        msg_pose.pose.orientation.w = quatation[3]
        # 发布消息

        msg_odom.header = Header()
        msg_odom.header.stamp = self.get_clock().now().to_msg()  # 当前时间戳
        msg_odom.header.frame_id = 'world'
        msg_odom.child_frame_id= 'laser_link'

        # 设置机器人的位置（pose）
        msg_odom.pose.pose = Pose()
        msg_odom.pose.pose.position.x = local_p[0]  # X坐标
        msg_odom.pose.pose.position.y = local_p[1]  # Y坐标
        msg_odom.pose.pose.position.z = local_p[2]  # Z坐标
        msg_odom.pose.pose.orientation.x = quatation[0]  # 四元数的X部分
        msg_odom.pose.pose.orientation.y = quatation[1]  # 四元数的Y部分
        msg_odom.pose.pose.orientation.z = quatation[2]  # 四元数的Z部分
        msg_odom.pose.pose.orientation.w = quatation[3]  # 四元数的W部分

        # 设置机器人的速度（twist）
        msg_odom.twist.twist = Twist()
        msg_odom.twist.twist.linear.x = velocity[0]  # 线速度，沿X轴
        msg_odom.twist.twist.linear.y = velocity[1]  # 线速度，沿Y轴
        msg_odom.twist.twist.linear.z = velocity[2]  # 线速度，沿Z轴
        msg_odom.twist.twist.angular.x = angular_velocity[0]  # 角速度，绕X轴
        msg_odom.twist.twist.angular.y = angular_velocity[1]  # 角速度，绕Y轴
        msg_odom.twist.twist.angular.z = angular_velocity[2]  # 角速度，绕Z轴
        # self.tf_broadcaster.sendTransform(msg)
        # msg_pose.pose.orientation.x = 0.0
        # msg_pose.pose.orientation.y = 0.0
        # msg_pose.pose.orientation.z = 0.0
        # msg_pose.pose.orientation.w = 1.0


        self.publisher_.publish(msg)
        self.publisher_pos.publish(msg_pose)
        self.publisher_odom.publish(msg_odom)

        msg = TransformStamped()
        msg.header.stamp = self.get_clock().now().to_msg()
        # msg.header.stamp = self.vehicle_rgbd.header.stamp #self.get_clock().now().to_msg() #这句不能有，不然Slam出问题
        msg.header.frame_id = "world"
        msg.child_frame_id = "laser_link"
        msg.transform.translation.x = local_p[0]
        msg.transform.translation.y = local_p[1]
        msg.transform.translation.z = local_p[2]
        msg.transform.rotation.w = quatation[3]
        msg.transform.rotation.x = quatation[0]
        msg.transform.rotation.y = quatation[1]
        msg.transform.rotation.z = quatation[2]
        self.tf_broadcaster.sendTransform(msg)
        
        # 打印日志
        #self.get_logger().info(f"Publishing velocity: {velocity}, position: {position}")

    # 计算 CRC16 校验
    def calc_crc(self, data: bytes) -> int:
        crc = 0xFFFF  # 初始值
        for byte in data:
            crc ^= byte  # 将数据字节与CRC异或
            for _ in range(8):  # 对每一位进行处理
                if (crc & 0x0001):  # 如果最低位为1
                    crc = (crc >> 1) ^ 0xA001  # 右移一位，与多项式A001异或
                else:
                    crc >>= 1  # 否则仅右移一位
        return crc & 0xFFFF
    
    def calc_sum(self, data: bytes):
        sum = 0x00
        for byte in data:
            sum += byte
        return sum & 0xFF

    # 解析报文
    def parse_message(self, data: bytes):
        idx = 0

        # 解析报文头
        if data[idx] != HEAD:
            raise ValueError("Invalid header")
        idx += 1

        # 解析数据ID
        data_id = data[idx]
        idx += 1
        # print(f"Data ID: {data_id}")

        # 解析数据域长度
        data_len = data[idx]
        idx += 1
        # print(f"Data Length: {data_len}")

        # 解析数据域
        data_field = data[idx:idx + data_len]
        idx += data_len
        # print(f"Data Field: {data_field.hex()}")

        # 解析 CRC 校验
        crc_received = struct.unpack(">H", data[idx:idx + 2])[0]

        # 校验 CRC
        crc_calculated = self.calc_crc(data[:-2])  # CRC 是不包括最后两位的
        if crc_received != crc_calculated:
            raise ValueError("CRC check failed")

        return data_id, data_field
    
    def euler_to_quaternion(self, pitch, roll, yaw):
        # 将角度转换为弧度
        pitch = math.radians(pitch)
        roll = math.radians(roll)
        yaw = math.radians(yaw)

        # 计算四元数的各个部分
        cy = math.cos(yaw * 0.5)
        sy = math.sin(yaw * 0.5)
        cp = math.cos(pitch * 0.5)
        sp = math.sin(pitch * 0.5)
        cr = math.cos(roll * 0.5)
        sr = math.sin(roll * 0.5)

        w = cr * cp * cy + sr * sp * sy
        x = sr * cp * cy - cr * sp * sy
        y = cr * sp * cy + sr * cp * sy
        z = cr * cp * sy - sr * sp * cy

        return w, x, y, z

    # 解析输出状态信息
    def parse_output_status(self, data_field):
        # 假设报文格式和字段顺序一致
        if data_field == None:
            print("Received No Data")
            return

        data = data_field
        idx = 0
        # Head
        head = struct.unpack(">H", data[idx:idx+2])[0]
        #print(f"frame head: {head}")
        idx += 2

        # 年/月/日
        utc_year = struct.unpack(">H", data[idx:idx+2])[0]
        utc_month = struct.unpack(">B", data[idx+2:idx+3])[0]
        utc_day = struct.unpack(">B", data[idx+3:idx+4])[0]
        idx += 4

        # 时/分/秒/毫秒
        utc_hour = struct.unpack(">B", data[idx:idx+1])[0]
        utc_min = struct.unpack(">B", data[idx+1:idx+2])[0]
        utc_second = struct.unpack(">B", data[idx+2:idx+3])[0]
        utc_mmsec = struct.unpack(">H", data[idx+3:idx+5])[0]
        # print(f"UTC Date: {utc_year}-{utc_month}-{utc_day} {utc_hour}:{utc_min}:{utc_second}.{utc_mmsec}")
        idx += 5


        focus_lon = struct.unpack(">i", data[idx:idx+4])[0]
        # print(f"focus point longitude: {focus_lon/1e7}")
        idx += 4

        focus_lat = struct.unpack(">i", data[idx:idx+4])[0]
        # print(f"focus point latitude: {focus_lat/1e7}")
        idx += 4

        drone_lon = struct.unpack(">i", data[idx:idx+4])[0]
        #print(f"drone longitude: {drone_lon/1e7}--{drone_lon}")
        idx += 4

        drone_lat = struct.unpack(">i", data[idx:idx+4])[0]
        #print(f"drone latitude: {drone_lat/1e7}--{drone_lat}")
        idx += 4

        focus_alt = struct.unpack(">i", data[idx:idx+4])[0]
        # print(f"focus point altitude: {focus_alt/1e2}m")
        idx += 4

        drone_alt = struct.unpack(">i", data[idx:idx+4])[0]
        # print(f"drone altitude: {drone_alt/1e2}m--{drone_alt}")
        idx += 4

        

        east_vel = struct.unpack(">i", data[idx:idx+4])[0]
        # print(f"east velocity: {east_vel/1e2}m/s")
        idx += 4

        north_vel = struct.unpack(">i", data[idx:idx+4])[0]
        # print(f"north velocity: {north_vel/1e2}m/s")
        idx += 4

        up_vel = struct.unpack(">i", data[idx:idx+4])[0]
        # print(f"up velocity: {up_vel/1e2}m/s")
        idx += 4

        roll = struct.unpack(">h", data[idx:idx+2])[0]
        #print(f"roll: {roll/1e2}")
        idx += 2

        pitch = struct.unpack(">h", data[idx:idx+2])[0]
        #print(f"pitch: {pitch/1e2}")
        idx += 2

        yaw = struct.unpack(">h", data[idx:idx+2])[0]
        #print(f"yaw: {yaw/1e2}")
        idx += 2

        angular_x = struct.unpack(">h", data[idx:idx+2])[0]
        idx += 2

        angular_y = struct.unpack(">h", data[idx:idx+2])[0]
        idx += 2

        angular_z = struct.unpack(">h", data[idx:idx+2])[0]
        idx += 2

        recieved_cal_sum = struct.unpack(">B", data[idx:idx+1])[0]
        cal_sum = self.calc_sum(data[:-3])
        if cal_sum != recieved_cal_sum:
            raise ValueError("Sum check failed")

        if(self.origin_alt==-1.0 and self.origin_lat==-1.0 and self.origin_lon==-1.0):
            self.origin_lon = drone_lon
            self.origin_lat = drone_lat
            self.origin_alt = drone_alt

        longitude = (drone_lon/1e7 - self.origin_lon/1e7) * (EARTH_RADIUS + self.origin_alt/1e2) / 180 * math.pi
        latitude = (drone_lat/1e7 - self.origin_lat/1e7) * (EARTH_RADIUS + self.origin_alt/1e2) / 180 * math.pi
        altitude = (drone_alt/1e2-self.origin_alt/1e2)
        diff_roll = (roll)/1e2
        diff_pitch = (pitch)/1e2
        diff_yaw = (yaw)/1e2
        w,x,y,z = self.euler_to_quaternion(diff_pitch, diff_roll, diff_yaw)
        #print(f"origin x--y--z: {diff_pitch}--{diff_roll}--{diff_yaw}")

        
        #print(f"diff: {longitude}--{latitude}")

        # publish the topic
        velocity = [east_vel/1e2, north_vel/1e2, up_vel/1e2]
        angular_velocity = [angular_x/1e2, angular_y/1e2, angular_z/1e2]
        position = [drone_lon/1e7, drone_lat/1e7, drone_alt/1e2]
        local_p = [longitude, latitude, altitude/1.0]
        quatation = [x,y,z,w]
        print(f"now lon--lat--alt: {position[0]}--{position[1]}--{position[2]}")
        print(f"origin pitch--roll--yaw: {diff_pitch}--{diff_roll}--{diff_yaw}")
        #print(f"origin vx--vy--vz: {velocity[0]}--{velocity[1]}--{velocity[2]}")

        self.publish_data(position, velocity, angular_velocity, local_p, quatation)

        # #回传凌云系统
        # type = 2
        # uav_id = self.droneid
        # longitude = position[0]
        # latitude = position[1]
        # altitude = position[2]
        # x_vel = velocity[0]
        # y_vel = velocity[1]
        # z_vel = velocity[2]
        # yaw = diff_yaw
        # pitch = diff_pitch
        # roll = diff_roll

        # status_data = struct.pack(
        #         'idi9d',
        #         type, uav_id, longitude, latitude, altitude, x_vel, y_vel, z_vel, roll, pitch, yaw
        #     )
        # self.sock.sendto(status_data, self.server_address)
        
        # curr_time = time.time()
        # # if(curr_time-self.last_time>1/self.frequency):
        # if not self.isSend:
        #     self.isSend = True
        #     self.last_time = curr_time
        #     mode_type = 1
        #     t = 1

        #     uavIndex = 0
        #     uavName = "UAV1"
        #     model = 1
        #     node = 1
        #     camp = 'Redada'
        #     longitude = drone_lon/1e7
        #     latitude = drone_lat/1e7
        #     altitude = drone_alt/1e2
        #     x_vel = velocity[0]
        #     y_vel = velocity[1]
        #     z_vel = velocity[2]
        #     yaw = diff_yaw
        #     pitch = diff_pitch
        #     roll = diff_roll
        #     jamSwitch = True
        #     investigateSwitch = False
        #     detectSwitch = True
        #     castSwitch = False
        #     a = True
        #     b = False
        #     c = True
        #     d = False

        #     colonyID = 897
        #     subcolconyID = 3
        #     current_num = 8
        #     formation_type = 2
        #     taskID = 1
        #     taskType = 1
        #     isFinish = False

        #     status_data = struct.pack(
        #         'iiiiii32siiii32sddddddddd????????',
        #         mode_type, t, colonyID, subcolconyID, formation_type, uavIndex, uavName.encode('ascii'), taskID, taskType, model, node, camp.encode('ascii'),
        #         longitude, latitude, altitude, x_vel, y_vel, z_vel, roll, pitch, yaw,
        #         jamSwitch, investigateSwitch, detectSwitch, castSwitch,
        #         a, b, c, d
        #     )
        #     #print(status_data.hex())
        #     colony_data = struct.pack(
        #         'iiiiiiii?',
        #         mode_type, t,colonyID, subcolconyID, current_num, formation_type, taskID, taskType, isFinish
        #     )
        #     test_data = struct.pack(
        #         'iiiidddd32s',
        #         1,2,3,4,5.0,6.0,7.0,8.0,'abcd'.encode('ascii')
        #     )
        #     self.sock.sendto(colony_data, self.server_address)
        #     #self.sock.sendto(test_data, ('127.0.0.1', 12345))
        #     self.sock.sendto(status_data, self.server_address)


    # 打开串口读取数据并解析
    def read_and_parse_serial_data(self, port='/dev/ttyACM1', baudrate=115200):
        
        try:
            with serial.Serial(port, baudrate, timeout=1) as ser:
                print("Waiting for data...")
                while True:
                    # 初始化接收缓冲区
                    buffer = bytearray()

                    while True:
                        
                        # 检查串口是否有数据
                        if ser.in_waiting > 0:
                            byte = ser.read(1)
                            #print("Get data...")
                            # 如果接收到的字节是报文头（0xA5），则开始读取报文
                            if len(buffer) == 0 and byte == bytes([HEAD]):
                                buffer.append(byte[0])  # 添加报文头
                                # print(f"Header found, starting to collect data...")

                            elif len(buffer) > 0:
                                buffer.append(byte[0])

                            # 一旦接收到完整的100字节报文，开始解析
                            if len(buffer) >3 and len(buffer) == buffer[2]+5:
                                # print(f"Received full message: {buffer.hex()}")
                                try:
                                    # 解析报文
                                    self.data_id, self.data_field = self.parse_message(buffer)
                                    self.parse_output_status(self.data_field)
                                except ValueError as e:
                                    print(f"Error while parsing data: {e}")
                                
                                # 清空缓冲区，等待下一个报文
                                buffer.clear()
                        #time.sleep(0.05)  # 小延时，避免 CPU 占用过高
        except serial.SerialException:
            while(True):
                print("使用设定值")
                velocity = [0.0, 0.0, 0.0]
                angular_velocity = [0.0, 0.0, 0.0]
                position = [self.setlon, self.setlat, self.setalt]
                local_p = [0.0, 0.0, 0.0]
                quatation = [0.0, 0.0, 0.0, 1.0]
                print(f"now lon--lat--alt: {position[0]}--{position[1]}--{position[2]}")
                #print(f"origin vx--vy--vz: {velocity[0]}--{velocity[1]}--{velocity[2]}")

                self.publish_data(position, velocity, angular_velocity, local_p, quatation)

                # #回传凌云系统
                # type = 2
                # uav_id = self.droneid
                # longitude = position[0]
                # latitude = position[1]
                # altitude = position[2]
                # x_vel = velocity[0]
                # y_vel = velocity[1]
                # z_vel = velocity[2]
                # yaw = 0.0
                # pitch = 0.0
                # roll = 0.0

                # status_data = struct.pack(
                #         'ii9d',
                #         type, uav_id, longitude, latitude, altitude, x_vel, y_vel, z_vel, roll, pitch, yaw
                #     )
                # self.sock.sendto(status_data, self.server_address)
                time.sleep(0.5)
        except KeyboardInterrupt:
            print("closeing socket...")
        finally:
            self.sock.close()
            print("Scoked closed")

def main(args=None):
    rclpy.init(args=args)
    node = PublisherNode()
    port = ('/dev/ttyACM'+str(sys.argv[1]))
    print(port)
    node.read_and_parse_serial_data(port)
    
    
if __name__ == "__main__":
    main()
